/*
 * To change this template, choose Tools | Templates
 * and open the template in the editor.
 */
package modes;

import main.ColorSensorChew;
import main.Engine;
import main.SensorEngine;
import main.SonicSensor;
import navigation.Navigator;

/**
 * siehe run
 * @author Max
 */
public class ModeBlack extends AMode
{

    private int countWhite = 0;
    private final int MAX_COUNT_WHITE = 40;
    private int currentMaxWhite = MAX_COUNT_WHITE / 2;
    private int failureCount = 0;

    /**
     * Constructor for black mode
     *
     * @param en Engine
     * @param r SensorEngine
     * @param c ColorSensor
     * @param s SonicSensor
     * @param nav Navigator
     */
    public ModeBlack(Engine en, SensorEngine r, ColorSensorChew c, SonicSensor s, Navigator nav)
    {
        super(en, r, c, s, nav);
    }

    /**
     * Follows the black line until it reads an other color than white or black.
     * If no black can be found black mode will be left.
     * 
     * @return next mode
     * @throws InterruptedException
     */
    public Mode run() throws InterruptedException
    {
        Mode mode = Mode.BLACK;
        engine.setSpeed(350);
        do
        {

            rotationSensor.rotateColorSensor();
            color.readColor();

            if (color.isColorBlack())
            {
                actionBlack();
            } else if (color.isColorWhite())
            {
                actionWhite();
            } else if (color.isColorBlue())
            {

                mode = Mode.BLUE;
            } else if (color.isColorGreen())
            {
                mode = Mode.GREEN;
            } else if (color.isColorRed())
            {
                mode = Mode.RED;
            } else if (color.isColorYellow())
            {
                mode = Mode.YELLOW;
            }
            if (sonic.isObject() && color.isColorBlack())
            {
                mode = Mode.FOUND_OBJECT;
            }
            mode = checkDeadEnd(mode);
        } while (mode == Mode.BLACK);
        resetBlack();
        return mode;
    }

    private void actionWhite() throws InterruptedException
    {
        if (rotationSensor.isMiddle())
        {
            rotationSensor.setRotating(false);
            if (countWhite == 0)
            {
                if (failureCount == 0)
                {
                }
                engine.switchRotation();
                failureCount++;

            } else if (countWhite >= currentMaxWhite)
            {
                engine.switchRotation();
                countWhite = 0;
                failureCount++;
                failureMode();

            }
            countWhite++;
        } else if (rotationSensor.isLeft())
        {
            engine.driveRight(70);

        } else if (rotationSensor.isRight())
        {
            engine.driveLeft(70);

        }
    }

    private void actionBlack()
    {
        rotationSensor.setRotating(true);
        resetBlack();
        if (rotationSensor.isLeft())
        {
            engine.driveLeft(70);

        } else if (rotationSensor.isRight())
        {
            engine.driveRight(70);

        } else
        {
            engine.forward();
        }
    }

    private void resetBlack()
    {
        failureCount = 0;
        countWhite = 0;
        currentMaxWhite = MAX_COUNT_WHITE / 2;
    }

    private void failureMode()
    {
        if (failureCount > 2)
        {
            currentMaxWhite += 30;
        } else
        {
            currentMaxWhite = MAX_COUNT_WHITE;
        }
    }

    private Mode checkDeadEnd(Mode mode)
    {
        if (failureCount > 4)
        {
            int ex = 0;
            if (engine.isRotateRight())
            {
                ex = 15;
            }
            for (int i = 0; i < (currentMaxWhite) / 2 + ex; i++)
            {
                color.readColor();
                if (color.isColorBlack())
                {
                    engine.stop();
                }
            }

            engine.stop();
            mode = Mode.YELLOW_WITHOUT_CHECK;
        }
        return mode;
    }
}
